#include <math.h>
#include <chassis_controller/kinematics.h>
#include <chassis_controller/kinematics_diff.h>

namespace chassis_controller
{

  DiffKinematics::DiffKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):
    Kinematics(node)
  {}
  
  void DiffKinematics::init(int pivot_num, pid_param steer_pid_param, SpeedLimiter limiter_liner, SpeedLimiter limiter_ang, SpeedLimiter limiter_steer) // PID instantiation
  {
    liner_setpoint_.resize(pivot_num);
    std::fill(liner_setpoint_.begin(), liner_setpoint_.end(), 0.0);
    angle_setpoint_.resize(pivot_num);
    std::fill(angle_setpoint_.begin(), angle_setpoint_.end(), 0.0);
    angle_setpoint_last_valid.resize(pivot_num);
    std::fill(angle_setpoint_last_valid.begin(), angle_setpoint_last_valid.end(), 0.0);
    limiter_liner_.resize(pivot_num);
    last0_cmd_liner_.resize(pivot_num);
    std::fill(last0_cmd_liner_.begin(), last0_cmd_liner_.end(), 0.0);
    last1_cmd_liner_.resize(pivot_num);
    std::fill(last1_cmd_liner_.begin(), last1_cmd_liner_.end(), 0.0);
    start_time_ = node_->now().seconds();
  }

  void DiffKinematics::updateParam(int pivot_num, pid_param steer_pid_param)
  {
  }
  void DiffKinematics::getCommand(double vx, double vy, double va)
  {
    setpoint_.x = vx;
    setpoint_.y = vy;
    setpoint_.a = va;
    // DEBUG_STREAM_OUT("current velocity setpoint :" << setpoint_.x << setpoint_.y << setpoint_.a);
  }

  void DiffKinematics::getVelocity(double vx, double vy, double va)
  {
    current_vel_.x = vx;
    current_vel_.y = vy;
    current_vel_.a = va;
    // DEBUG_STREAM_OUT("current velocity :" << current_vel_.x << current_vel_.y << current_vel_.a);
  }

  void DiffKinematics::update(std::vector<Spivot_value> &pivot_value,
                              std::vector<Spivot_param> &pivot_param,
                              std::vector<Spivot_setpoint> &pivot_setpoint, const rclcpp::Duration &period,
                              bool ready_move)
  {
    pivots_num_ = pivot_value.size();
    bool stop_req = false;
    drive_request_ = abs(setpoint_.x) > 0.00005 || abs(setpoint_.y) > 0.00005 || abs(setpoint_.a) > 0.00005;
    standstill_ = abs(current_vel_.x) < 0.005 && abs(current_vel_.y) < 0.005 && abs(current_vel_.a) < 0.005;
    if (!drive_request_)
      movement_ = false;

    if (drive_request_)
    {
      start_time_ = node_->now().seconds();
    }

    double time_elapse = node_->now().seconds() - start_time_;
    if (time_elapse < 2 && drive_request_)
      braker_open_ = true;
    else
      braker_open_ = false;
   
    double temp_vl = setpoint_.x - setpoint_.a * pivot_param[0].radius_ringmount;
    double temp_vr = setpoint_.x + setpoint_.a * pivot_param[0].radius_ringmount;
    // DEBUG_STREAM_OUT("radius_ringmount is: " << pivot_param[0].radius_ringmount);
    pivot_setpoint[0].joint_setpoint[0].velocity_setpoint = temp_vl;
    pivot_setpoint[0].joint_setpoint[1].velocity_setpoint = temp_vr;
    // DEBUG_STREAM_OUT("left velocity setpoint: " << temp_vl << " , right velocity setpoint: " << temp_vr);
  }

  void DiffKinematics::setCommand(std::vector<Spivot_value> &pivot_value,
                                  std::vector<Spivot_param> &pivot_param,
                                  std::vector<Spivot_setpoint> &pivot_setpoint,
                                  const rclcpp::Duration &period) 
  {

    std::vector<double> MperSec2RadPerSec;
    MperSec2RadPerSec.resize(2);
    MperSec2RadPerSec[0]= 2.0 / pivot_param[0].joint_param[0].wheel_diameter;
    MperSec2RadPerSec[1]= 2.0 / pivot_param[0].joint_param[1].wheel_diameter;
    // DEBUG_STREAM_OUT("MperSec2RadPerSec : "<<MperSec2RadPerSec[0] << "," << MperSec2RadPerSec[1]);
    for (int j = 0; j < 2; ++j){
      std::string pivot_joint_name = pivot_param[0].joint_param[j].name;
      assert (!pivot_joint_name.empty());

      if (pivot_param[0].joint_param[j].drive_mode == TRACTION) { // assign angle and velocity if active pivot
        pivot_param[0].joint_param[j].handle.joint_command_interfaces[0].get().set_value(
          pivot_setpoint[0].joint_setpoint[j].velocity_setpoint * MperSec2RadPerSec[j]);
        // DEBUG_OUT("[DiffKinematics] jointHandle[%s] set pivot velocity: %.4f", 
        //   pivot_joint_name.c_str(), pivot_setpoint[0].joint_setpoint[j].velocity_setpoint);
      }
      else{
        DEBUG_OUT_THROTTLE(get_clock(), 500, "drive_mode err!!");
      }
    }
  }

  void DiffKinematics::setSteerCmdId(uint64_t steer_cmd_id)
  {
  }

  bool DiffKinematics::getSteeringFlag()
  {
    return false;
  }

  bool DiffKinematics::getSteerErrFlag()
  {
    return false;
  }

  bool DiffKinematics::getBrakerFlag()
  {
    return braker_open_;
  }

  uint64_t DiffKinematics::getSteerCmdId()
  {
    return 0;
  }

}
